/*
 * @Description: 前端具体实现
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:05:00
 * @LastEditTime: 2021-11-09 20:15:45
 * @LastEditors: Sang Hao
 */
#ifndef LIDAR_SLAM_MAPPING_FRAONT_END_FRAONT_END_HPP_
#define LIDAR_SLAM_MAPPING_FRAONT_END_FRAONT_END_HPP_

#include <deque>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>

#include "lidar_slam/sensor_data/cloud_data.hpp"
#include "lidar_slam/models/registration/ndt_registration.hpp"
#include "lidar_slam/models/cloud_filter/voxel_filter.hpp"
#include "lidar_slam/models/cloud_filter/no_filter.hpp"

namespace lidar_slam {
class FrontEnd {
public:
	/* 定义帧数据结构，点云 + pose */
	struct Frame {
		Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
		CloudData cloud_data;
	};

private:
	/* data */
	std::string data_path_ = "";

	std::shared_ptr<CloudFilterInterface> frame_filter_ptr_;
	std::shared_ptr<CloudFilterInterface> local_map_filter_ptr_;
	std::shared_ptr<RegistrationInterface> registration_ptr_;

	Frame current_frame_;
	CloudData::CLOUD_PTR local_map_ptr_;
	/* deque,该变量实现了成员函数之间的参数传递 */
	std::deque<Frame> local_map_frames_;
	
	Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();

	float key_frame_distance_ = 1.0;
	int local_frame_num_ = 10;

private:
	bool InitWithConfig();
	bool InitParam(const YAML::Node& config_node);
	bool InitRegistration(	std::shared_ptr<RegistrationInterface>& registration_ptr, 
							const YAML::Node& config_node);
	bool InitFilter(std::string filter_user, 
					std::shared_ptr<CloudFilterInterface>& filter_ptr, 
					const YAML::Node& config_node);
	bool UpdateWithNewFrame(const Frame& new_key_frame);

public: 
	FrontEnd(/* args */);
	bool Update(const CloudData& cloud_data, Eigen::Matrix4f& cloud_pose);
	bool SetInitPose(const Eigen::Matrix4f& init_pose);

};
}

#endif